#include "ros_mec_odom.h"


void mySigIntHandler(int sig)
{
	ros::shutdown();
}

ros_mec_odom::ros_mec_odom()
{
	x = y = th = vx = vy = vth = dt = 0.0;

	ros::NodeHandle nh_private("~");
	nh_private.param<std::string>("robot_frame_id", this->robot_frame_id, "base_link");
	nh_private.param<std::string>("gyro_frame_id", this->gyro_frame_id, "gyro_link");
	nh_private.param<double>("filter_Vx_match", this->filter_Vx_match, 1.0f); 
	nh_private.param<double>("filter_Vy_match", this->filter_Vy_match, 1.0f); 
	nh_private.param<bool>("publish_imu", this->publish_imu, false); 

	this->odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
	this->imu_pub = n.advertise<sensor_msgs::Imu>("imu", 50);
	this->imu_sub = n.subscribe("/imu_data", 20, &ros_mec_odom::imu_callback, this);
	this->mec_sub = n.subscribe("/mec_car", 100, &ros_mec_odom::mec_callback, this);
}

//imu回调函数
void ros_mec_odom::imu_callback(const sensor_msgs::Imu &imu)
{
	this->vth = imu.angular_velocity.z;
	this->imu_msg = imu;
}

//底盘通讯回调函数
void ros_mec_odom::mec_callback(const ros_mec_msg::york &msg)
{
	this->current_time = ros::Time::now();
	if(!frame_num) last_time = ros::Time::now();
	this->dt = (current_time - last_time).toSec();
	this->frame_num++;

	this->vx =  msg.Vx * filter_Vx_match;
	this->vy =  msg.Vy * filter_Vy_match;

	double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
	double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
	double delta_th = vth * dt;
	x += delta_x;
	y += delta_y;
	th += delta_th;

	odom_publisher();

	this->last_time = current_time;
}

//发布里程计数据
void ros_mec_odom::odom_publisher()
{
	geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

	nav_msgs::Odometry odom;
	odom.header.stamp = ros::Time::now();;
	odom.header.frame_id = "odom";

	odom.pose.pose.position.x = x;
	odom.pose.pose.position.y = y;
	odom.pose.pose.orientation = odom_quat;

	odom.child_frame_id = this->robot_frame_id;
	odom.twist.twist.linear.x =  this->vx;
	odom.twist.twist.linear.y =  this->vy;
	odom.twist.twist.angular.z = this->vth;
	
	if(this->vx == 0)
	{
		memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2));
		memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
	}
	else
	{
    		memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance));
    		memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));
	}				

	if(this->publish_imu)
	{
		sensor_msgs::Imu imu;
		imu = this->imu_msg;
		imu.header.stamp = ros::Time::now();;
		imu.header.frame_id = this->gyro_frame_id;
		imu.orientation = odom_quat;
		imu_pub.publish(imu);
	}

 	odom_pub.publish(odom);
}

int main(int argc,char **argv)
{
	ros::init(argc, argv, "publish_odom");
	signal(SIGINT, mySigIntHandler);

	ros_mec_odom Publish_Odom; 
	ros_mec_odom Publish_Imu; 
	ros::spin();
	return 0;
}
